#include"visual_odom/map.h"

namespace test_visual_odom{

void Map::insertKeyFrame(Frame::Ptr p_frame){
    p_current_frame_ = p_frame;
    //按道理是新发现的关键帧才会才入的，但是，为了鲁棒性，还是需要进行判断
    if(keyframes_.find(p_frame->keyframe_id_)==keyframes_.end()){
        keyframes_.insert(make_pair(p_frame->keyframe_id_, p_frame));
        active_keyframes_.insert(make_pair(p_frame->keyframe_id_, p_frame));
    } else {  //更相信当前发现的关键帧
        keyframes_[p_frame->keyframe_id_] = p_frame;
        active_keyframes_[p_frame->keyframe_id_] = p_frame;
    }

    if (active_keyframes_.size() > num_active_keyframes_) {
        removeNotGoodKeyframe();
    }        
}

void Map::removeNotGoodKeyframe(){
    // 寻找与当前帧最近与最远的两个关键帧
    double max_dis = 0, min_dis = 9999;
    double max_kf_id = 0, min_kf_id = 0;
    auto Twc = p_current_frame_->getPose().inverse();
    for (auto& kf : active_keyframes_) {
        if (kf.second == p_current_frame_) continue;  //由于是指针，可以比较指针是否相同
        auto dis = (kf.second->getPose() * Twc).log().norm();  //
        if (dis > max_dis) {
            max_dis = dis;
            max_kf_id = kf.first;
        }
        if (dis < min_dis) {
            min_dis = dis;
            min_kf_id = kf.first;
        }
    }

    const double min_dis_th = 0.2;  // 最近阈值
    Frame::Ptr frame_to_remove = nullptr;
    if (min_dis < min_dis_th) {
        // 如果存在很近的帧，优先删掉最近的
        frame_to_remove = keyframes_.at(min_kf_id);
    } else {
        // 删掉最远的
        frame_to_remove = keyframes_.at(max_kf_id);
    }

    ROS_DEBUG_STREAM("remove keyframe " << frame_to_remove->keyframe_id_);
    // remove keyframe and landmark observation
    active_keyframes_.erase(frame_to_remove->keyframe_id_);
    for (auto feat : frame_to_remove->feature_points_left_) {
        auto mp = feat->p_landmark_.lock();  //如果这个特征点对应有landmark的话，就可以lock()获得其指针
        if (mp) {
            mp->removeFeaturePoint2D(feat);
        }
    }
    for (auto feat : frame_to_remove->feature_points_right_) {
        if (feat == nullptr) continue;
        auto mp = feat->p_landmark_.lock();
        if (mp) {
            mp->removeFeaturePoint2D(feat);
        }
    }

    removeLandMarkNotObserved();
}

void Map::insertLandMark(LandMark::Ptr landmark) {
    if (landmarks_.find(landmark->id_) == landmarks_.end()) {
        landmarks_.insert(make_pair(landmark->id_, landmark));
        active_landmarks_.insert(make_pair(landmark->id_, landmark));
    } else {  //更相信新发现的landmark
        landmarks_[landmark->id_] = landmark;
        active_landmarks_[landmark->id_] = landmark;
    }
}

void Map::removeLandMarkNotObserved() {
    int cnt_landmark_removed = 0;
    for (auto iter = active_landmarks_.begin();
         iter != active_landmarks_.end();) {
        if (iter->second->observed_times_ == 0) {
            iter = active_landmarks_.erase(iter);
            cnt_landmark_removed++;
        } else {
            ++iter;
        }
    }
    ROS_DEBUG_STREAM("Removed " << cnt_landmark_removed << " active landmarks");
}

}